#include "hardware_driver/MotorCan.hpp"
#include <cstdint>
#include <cstring>
#include <functional>

namespace HardWareDriver
{
bool MotorCan::InitDevice(std::shared_ptr<CanServer> canBusInst, MotorConfig cfg)
{
    if (canBusInst == nullptr)
        return false;
    this->bus = canBusInst;
    this->cfg = cfg;
    bus->RegeistCallBack(cfg.address, bind(&MotorCan::OnReceiveCallBack, this, std::placeholders::_1));
    return true;
}

float MotorCan::GetRPM()
{
    return static_cast<float>(erpm.load()) / cfg.poleCount;
}

bool MotorCan::RequestRPM()
{
    return bus->Send(cfg.address, uint16_t{0x0f01});
}

bool MotorCan::SetRPM(float value)
{
    int32_t erpm = value * cfg.poleCount;
    return bus->Send(cfg.address, uint8_t{02}, erpm);
}

bool MotorCan::SetPWM(float precent)
{
    uint16_t value = (precent * 1000);
    return bus->Send(cfg.address, uint8_t{03}, value);
}

bool MotorCan::SetCurrent(float mA)
{
    int16_t value = (mA / 10);
    return bus->Send(cfg.address, uint8_t{01}, value);
}

bool MotorCan::SendHeartBeat()
{
    return bus->Send(cfg.address, uint8_t{00});
}

MotorCan::MotorCan()
{
    erpm.store(0);
}

MotorCan::~MotorCan()
{
    if (bus != nullptr)
        bus->RemoveCallBack(cfg.address);
}

void MotorCan::OnReceiveCallBack(const can_frame &frame)
{
    if (frame.data[0] == 0x0f && frame.data[1] == 0x01 && frame.can_dlc == 6)
    {
        // erpm 4byte and in invers byte sequence
        int32_t buf = 0;
        buf = frame.data[2] << 24 | frame.data[3] << 16 | frame.data[4] << 8 | frame.data[5];
        erpm.store(buf);
        // std::cout << "RECEIVE ERPM" << std::endl;
    }
}
} // namespace HardWareDriver